#ifndef ROBORTS_DECISION_BUFF_BEHAVIOR_H
#define ROBORTS_DECISION_BUFF_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class BuffBehavior{
  public:
  BuffBehavior(ChassisExecutor* &chassis_executor,
		Blackboard* &blackboard,
	const std::string & proto_file_path) : chassis_executor_(chassis_executor),
                                                       blackboard_(blackboard) {
    if (!blackboard_->LoadBuffParam(proto_file_path)) {
      ROS_ERROR("%s can't open file", __FUNCTION__);
    }
  }

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }
/*
  bool LoadParam(const std::string &proto_file_path) {
    roborts_decision::DecisionConfig decision_config;
    if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) {
      return false;
    }
    for(int i=0;i<6;i++) {
      buff_position_[i].header.frame_id="map";
      buff_position_[i].pose.position.x=decision_config.buff_point(i).x();
      buff_position_[i].pose.position.y=decision_config.buff_point(i).y();
      buff_position_[i].pose.position.z=decision_config.buff_point(i).z();
      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw((decision_config.buff_point(i).roll(),
                                                              decision_config.buff_point(i).pitch(),
                                                              decision_config.buff_point(i).yaw());
      buff_position_[i].pose.orientation=quaternion;
     // buff_position_[i].pose.orientation.x()=quaternion.x();
   //buff_position_[i].pose.orientation.y=quaternion.y();
//buff_position_[i].pose.orientation.z=quaternion.z();
//buff_position_[i].pose.orientation.w=quaternion.w();
}
}*/

  void Run(int i) {
    if(i == 0) return;
    auto executor_state = Update();
    if (in_circle==0 && executor_state != BehaviorState::RUNNING && executor_state != BehaviorState::FAILURE) {
      geometry_msgs::PoseStamped buff_position = blackboard_->getbuffpositionfromblackboard(i);
      geometry_msgs::PoseStamped my_position = blackboard_->GetRobotMapPose();
      if(buff_position.pose.position.x>my_position.pose.position.x)
      {
        buff_position.pose.orientation.z = 0;
        buff_position.pose.orientation.w = 1;
      }
      else
      {
        buff_position.pose.orientation.w = 0;
        buff_position.pose.orientation.z = -1;
      }
      switch(count){
        case 0: 
          buff_position.pose.position.x = buff_position.pose.position.x - 0.23;
          buff_position.pose.position.y = buff_position.pose.position.y + 0.20;  // 左上角
          break;
        case 1: 
          buff_position.pose.position.x = buff_position.pose.position.x;
          buff_position.pose.position.y = buff_position.pose.position.y;  // 中间
          break;
        case 2: 
          buff_position.pose.position.x = buff_position.pose.position.x + 0.23;
          buff_position.pose.position.y = buff_position.pose.position.y - 0.20;  // 右下角
          break;
        case 3: 
          buff_position.pose.position.x = buff_position.pose.position.x + 0.23;
          buff_position.pose.position.y = buff_position.pose.position.y + 0.20;  // 右上角
          break;
        case 4: 
          buff_position.pose.position.x = buff_position.pose.position.x;
          buff_position.pose.position.y = buff_position.pose.position.y;  // 中间
          break;
        case 5: 
          buff_position.pose.position.x = buff_position.pose.position.x - 0.23;
          buff_position.pose.position.y = buff_position.pose.position.y - 0.20;  // 左下角
          break;
        default:  break;
      }
      count++;
      count = count % 6;
    
      chassis_executor_->Execute(buff_position, Behaviortype::BUFF);
    }
else if(in_circle||executor_state == BehaviorState::FAILURE)
{
if(in_circle==0)
    {
       begin_time = ros::Time::now();
     }
     in_circle=1;
     roborts_msgs::TwistAccel t;
      t.twist.angular.z = 1.57;
      chassis_executor_->Execute(t);
       if(ros::Time::now().sec-begin_time.sec>=4)in_circle=0;
}
  }



~BuffBehavior(){Cancel();}

private:
  ChassisExecutor* const chassis_executor_;
  Blackboard* const blackboard_;
  //geometry_msgs::PoseStamped buff_position_[8];
  int count = 0;
  int in_circle;
  ros::Time begin_time;
};
}
#endif